#include "common.h"

struct can1_rx0_buf_info can1_rx0_buf;
struct can1_rx1_buf_info can1_rx1_buf;

TX_MUTEX mutex_can_dev;

TX_THREAD     can1_rx0_thread_handle;
unsigned char can1_rx0_thread_stack[4096];

TX_THREAD     can1_rx1_thread_handle;
unsigned char can1_rx1_thread_stack[1024];

unsigned int pump_id_mark[4];

TX_THREAD     state_manage_thread_handle;
unsigned char state_manage_thread_stack[1024];

TX_THREAD     upload_operate_parm_thread_handle;
unsigned char upload_operate_parm_thread_stack[1024];

TX_SEMAPHORE semaphore_upload_operate_parm;
TX_TIMER     timer_upload_operate_parm;

VOID timer_upload_operate_parm_expiration_function(ULONG rsvd)
{
    tx_semaphore_put(&semaphore_upload_operate_parm);
}

void init_can_app_var(void)
{
    memory_set((unsigned char *)&can1_rx0_buf, 0, sizeof(can1_rx0_buf));
    memory_set((unsigned char *)&can1_rx1_buf, 0, sizeof(can1_rx1_buf));

    tx_mutex_create(&mutex_can_dev, NULL, TX_INHERIT);

    for (unsigned int j = 0; j < 4; j++)
        pump_id_mark[j] = j + 31;

    tx_semaphore_create(&semaphore_upload_operate_parm, NULL, 0);
    tx_timer_create(&timer_upload_operate_parm, 0, timer_upload_operate_parm_expiration_function, 0, 200, 200, TX_NO_ACTIVATE);
}

int get_pump_state_index(unsigned int id)
{
    int ret = -1;
    for (unsigned int i = 0; i < 4; i++) {
        if (id == pump_id_mark[i]) {
            ret = i;
            break;
        }
    }
    return ret;
}

void fill_operate_parm_report(void)
{
    // memory_copy(&operate_parm_report, &operate_parm, sizeof(operate_parm_report));

    operate_parm_report.power_operate_parm.power_state    = operate_parm.power_operate_parm.power_state;     // 单纤电源状态
    operate_parm_report.power_operate_parm.modul1_C_Out   = operate_parm.power_operate_parm.modul1_C_Out;    // 预放1输出电流
    operate_parm_report.power_operate_parm.modul1_V       = operate_parm.power_operate_parm.modul1_V;        // 预放1输出电压
    operate_parm_report.power_operate_parm.modul1_State   = operate_parm.power_operate_parm.modul1_State;    // 预放1状态
    operate_parm_report.power_operate_parm.modul2_C_Out   = operate_parm.power_operate_parm.modul2_C_Out;    // 预放2输出电流
    operate_parm_report.power_operate_parm.modul2_V       = operate_parm.power_operate_parm.modul2_V;        // 预放2输出电压
    operate_parm_report.power_operate_parm.modul2_State   = operate_parm.power_operate_parm.modul2_State;    // 预放2状态
    operate_parm_report.power_operate_parm.modul3_C_Out_L = operate_parm.power_operate_parm.modul3_C_Out_L;  // 主放1输出电流
    operate_parm_report.power_operate_parm.modul3_C_Out_H = operate_parm.power_operate_parm.modul3_C_Out_H;  // 主放1输出电流
    operate_parm_report.power_operate_parm.modul3_V       = operate_parm.power_operate_parm.modul3_V;        // 主放1输出电压
    operate_parm_report.power_operate_parm.modul3_State   = operate_parm.power_operate_parm.modul3_State;    // 主放1状态
    operate_parm_report.power_operate_parm.modul4_C_Out_L = operate_parm.power_operate_parm.modul4_C_Out_L;  // 主放1输出电流
    operate_parm_report.power_operate_parm.modul4_C_Out_H = operate_parm.power_operate_parm.modul4_C_Out_H;  // 主放1输出电流
    operate_parm_report.power_operate_parm.modul4_V       = operate_parm.power_operate_parm.modul4_V;        // 主放2输出电压
    operate_parm_report.power_operate_parm.modul4_State   = operate_parm.power_operate_parm.modul4_State;    // 主放2状态
    // operate_parm_report.power_operate_parm.version = operate_parm.power_operate_parm.version;               // 版本

    operate_parm_report.laser_operate_data = operate_parm.laser_operate_data;  // 版本

    // operate_parm_report.laser_operate_data.light_intensity = 1; // 光强
    // operate_parm_report.laser_operate_data.standby = 2;         // bit0:0：调试模式 1：正常工作  bit1：0：未撤收 1：已撤收  bit2：0:冷待机 1：热待机
    // operate_parm_report.laser_operate_data.fault_code1.all = 3; // 故障码1
    // operate_parm_report.laser_operate_data.fault_code2.all = 4; // 故障码2
    // operate_parm_report.laser_operate_data.fault_code3.all = 5; // 故障码3

    operate_parm_report.seedSrc_operate_data.seedState = operate_parm.seedSrc_operate_data.seedState;  // 子束状态 0:空闲 1:出强光 3:出1档弱光 5:出2档弱光
    operate_parm_report.seedSrc_operate_data.MOD       = operate_parm.seedSrc_operate_data.MOD;        // mod on/off
    operate_parm_report.seedSrc_operate_data.MT        = operate_parm.seedSrc_operate_data.MT;         // 模块温度
    operate_parm_report.seedSrc_operate_data.AST       = operate_parm.seedSrc_operate_data.AST;        // 工作状态 ok/异常
    operate_parm_report.seedSrc_operate_data.echo      = operate_parm.seedSrc_operate_data.echo;       // 工作状态 ok/异常

    operate_parm_report.power_operate_parm.modul1_State |= (modul1_State_Mask & 0x7D);
    operate_parm_report.power_operate_parm.modul2_State |= (modul2_State_Mask & 0x7D);
    operate_parm_report.power_operate_parm.modul3_State |= (modul3_State_Mask & 0x7D);
    operate_parm_report.power_operate_parm.modul4_State |= (modul4_State_Mask & 0x7D);

    /// 主激光
    operate_parm_report.laser_operate_data.standby &= 0b111;
    operate_parm_report.laser_operate_data.fault_code2.bit.laser_comm_err = 0;
}

void can1_rx1_thread(ULONG thread_input)
{
    unsigned int cmd              = 0;
    unsigned int id               = 0;
    unsigned int pump_state_index = 0;

    for (unsigned int i = 0; i < 4; i++) {
        pump_comm_timestamp[i] = tx_time_get();
    }

    while (1) {
        while (can1_rx1_buf.head != can1_rx1_buf.tail) {
            id               = (can1_rx1_buf.buf[can1_rx1_buf.head].exid >> 20) & 0xff;
            pump_state_index = get_pump_state_index(id);
            cmd              = can1_rx1_buf.buf[can1_rx1_buf.head].exid & 0xfff;

            switch (cmd) {
                case 50: {
                    break;
                }

                case 51: {
                    break;
                }

                case 52: {
                    break;
                }

                default:
                    break;
            }

            pump_comm_timestamp[pump_state_index] = tx_time_get();
            can1_rx1_buf.head++;
            if (CAN1_RX1_BUF_LEN <= can1_rx1_buf.head)
                can1_rx1_buf.head = 0;
        }

        // for (unsigned int j = 0; j < 4; j++)
        // {
        //   if ((1 == adaptor_parm.part.laser_module_adaptor_parm[j + 1].check_feature_2.part.laser_comm) &&
        //       (!(operate_parm.laser_operate_data[0].standby & (1 << 0)))) // 监控开启、不为调试模式
        //   {
        //     if (250 <= time_span(pump_comm_timestamp[j]))
        //     {
        //       operate_parm.laser_operate_data[j + 1].fault_code2.bit.laser_comm_err = 1;
        //     }
        //   }
        //   else
        //     operate_parm.laser_operate_data[j + 1].fault_code2.bit.laser_comm_err = 0;
        // }

        tx_thread_relinquish();
    }
}

void configurable_parm_to_flash_parm(void)
{
    configurable_ada_parm.avaliable.laser_parm.can_id = DEVICE_ID;

    flash_ada_parm.part.power_output_adaptor_parm = configurable_ada_parm.avaliable.power_parm;
    memory_copy(&flash_ada_parm.part.laser_module_adaptor_parm, &configurable_ada_parm.avaliable.laser_parm, sizeof(configurable_ada_parm.avaliable.laser_parm));
}

void flash_parm_to_configurable_parm(void)
{
    // 有空可以调整
    configurable_ada_parm.avaliable.power_parm = flash_ada_parm.part.power_output_adaptor_parm;

    memory_copy(&(configurable_ada_parm.avaliable.laser_parm.can_id), &(flash_ada_parm.part.laser_module_adaptor_parm.can_id), sizeof(configurable_ada_parm.avaliable.laser_parm));
}

void can_protocol_parse(struct can_message *msg)
{
    uint16_t               Can_Command = msg->exid & 0xfff;
    uint64_t               Buffer_Data = 0;
    unsigned int           src_id      = (msg->exid >> 20) & 0xff;
    Function_State_Typedef result      = 0;
    if (((msg->exid >> 12) & 0x00FF) == 0)  // 广播地址
    {
        Buffer_Data = (((uint64_t)msg->data.byte[0]) << 48) + (((uint64_t)msg->data.byte[1]) << 40) + (((uint64_t)msg->data.byte[2]) << 32) + (((uint64_t)msg->data.byte[3]) << 24) +
                      (((uint64_t)msg->data.byte[4]) << 16) + (((uint64_t)msg->data.byte[5]) << 8) + msg->data.byte[6];  // 获取命令的广播地址标识码
        if ((Buffer_Data & ((uint64_t)(1 << (DEVICE_ID - 3)))) == 0) {
            return;  // 广播地址标识位中未激活此子束，不做命令处理
        }
    } else if (((msg->exid >> 12) & 0x00FF) != DEVICE_ID)  // 广播地址
    {
        return;
    }

    switch (Can_Command) {
        case 0x00: {
            if (msg->data.byte[7] == 0x0F) {
                master_comm_err_cnt = 0;
                uperMonitor         = 0;
            } else if (msg->data.byte[7] == 0x8F) {
                master_comm_err_cnt = 0;
                uperMonitor         = 1;
            }

            break;
        }

        case (0x001):  // 复位操作
        {
            if ((msg->data.byte[7] != 0x0F) || (operate_parm.laser_operate_data.Laser_State == 0x01) || (operate_parm.laser_operate_data.Laser_State == 0x03) ||
                (operate_parm.laser_operate_data.Laser_State == 0x05) || (operate_parm.laser_operate_data.Laser_State == 0x06))  // 命令码不符  出光状态不能进行复位操作
            {
                Can_Command_Reply(msg, Failure);
            } else {
                if (Can_Reset_Command_Dipose() != Success) {
                    Can_Command_Reply(msg, Failure);  // 操作失败
                } else {
                    Can_Command_Reply(msg, Success);
                }
            }
            break;
        }

        case (0x002):  // 作战准备命令
        {
            if (((operate_parm.laser_operate_data.standby >> 1) & 0x01)  // 撤收状态或不为空闲状态
                || (operate_parm.laser_operate_data.Laser_State != 0x00)) {
                Can_Command_Reply(msg, Failure);
            } else {
                if (msg->data.byte[7] == 0x0F)  // 热待机
                {
                    if (Can_Ready_Command_Dipose(0x0F) != Success) {
                        Can_Command_Reply(msg, Failure);
                    } else {
                        Can_Command_Reply(msg, Success);
                    }
                } else if (msg->data.byte[7] == 0xF0)  // 冷待机
                {
                    if (Can_Ready_Command_Dipose(0xF0) != Success) {
                        Can_Command_Reply(msg, Failure);
                    } else {
                        Can_Command_Reply(msg, Success);
                    }
                } else  // 命令码不符
                {
                    Can_Command_Reply(msg, Failure);
                }
            }
            break;
        }

        case (0x003):  // 出光/停光操作
        {
            if ((operate_parm.laser_operate_data.standby >> 1) & 0x01)  // 撤收状态无法操作
            {
                Can_Command_Reply(msg, Failure);
            } else  // 非撤收状态
            {
                if (msg->data.byte[7] == 0x01)  // 停光
                {
                    if (0x06 == operate_parm.laser_operate_data.Laser_State)  // 正在出红光
                    {
                        HG_CTRL(OFF);
                        operate_parm.laser_operate_data.Laser_State = 0x00;
                        Can_Command_Reply(msg, Success);
                    } else {
                        if (Can_Close_Control_Command_Dipose() != Success)  // 停光操作
                        {
                            Can_Command_Reply(msg, Failure);
                        } else {
                            Can_Command_Reply(msg, Success);
                        }
                    }
                } else if ((msg->data.byte[7] == 0x02) || (msg->data.byte[7] == 0x04) || (msg->data.byte[7] == 0x05) || (msg->data.byte[7] == 0x03))  // 出强光、1级弱光、2级弱光、出红光
                {
                    if (operate_parm.laser_operate_data.Laser_State != 0x00)  // 子束非空闲状态无法操作
                    {
                        Can_Command_Reply(msg, Failure);
                    } else  // 子束为空闲状态
                    {
                        if ((msg->data.byte[7] == 0x02) || (msg->data.byte[7] == 0x04) || (msg->data.byte[7] == 0x05))  // 出强光、1级弱光、2级弱光
                        {
                            if (((operate_parm.laser_operate_data.standby >> 2) & 0x01))  // 热待机状态下允许出光操作
                            {
                                tx_mutex_get(&mutex_usart_dev, TX_WAIT_FOREVER);
                                result = Can_Open_Control_Command_Dipose(msg->data.byte[7]);  // 驱动源

                                tx_mutex_put(&mutex_usart_dev);
                                if (result != Success)  // 出光
                                {
                                    Can_Command_Reply(msg, Failure);
                                } else {
                                    if (msg->data.byte[7] == 0x02) {
                                        tx_queue_send(&queue_emit_success, &src_id, TX_NO_WAIT);
                                    }
                                    Can_Command_Reply(msg, Success);
                                }
                            } else {
                                Can_Command_Reply(msg, Failure);
                            }
                        } else  // 出红光
                        {
                            if (((operate_parm.laser_operate_data.standby >> 2) & 0x01))  // 热待机状态下允许出光操作
                            {
                                HG_CTRL(ON);
                                operate_parm.laser_operate_data.Laser_State = 0x6;
                                Can_Command_Reply(msg, Success);
                            } else {
                                Can_Command_Reply(msg, Failure);
                            }
                        }
                    }
                } else  // 命令不符
                {
                    Can_Command_Reply(msg, Failure);
                }
            }
            break;
        }

        case (0x004):  // 撤收命令
        {
            if ((msg->data.byte[7] != 0x0F) || (operate_parm.laser_operate_data.Laser_State != 0x00))  // 命令码不符、非空闲状态
            {
                Can_Command_Reply(msg, Failure);
            } else {
                /*功率输出设置为0*/
                operate_parm.laser_operate_data.light_intensity = 0;

                /*冷待机操作*/
                if (Success != Can_Ready_Command_Dipose(0xF0))  // 冷待机操作
                {
                    Can_Command_Reply(msg, Failure);
                } else {
                    /*切换置撤收状态*/
                    operate_parm.laser_operate_data.standby |= 0x01 << 1;
                    Can_Command_Reply(msg, Success);
                }
            }

            break;
        }

        case (0x005):  // 调试模式操作
        {
            if (msg->data.byte[7] == 0x0F)  // 开启调试
            {
                /*光源故障清除*/
                operate_parm.laser_operate_data.fault_code1.all = 0;
                operate_parm.laser_operate_data.fault_code2.all = 0;
                operate_parm.laser_operate_data.fault_code3.all = 0;

                /*监控点关闭*/
                for (unsigned int j = 0; j < 5; j++) {
                    adaptor_parm.part.laser_module_adaptor_parm.check_feature_1.all = 0;
                    adaptor_parm.part.laser_module_adaptor_parm.check_feature_2.all = 0;
                }

                brake_state = 0;
                EXTI->IMR &= ~(1 << 2);
                EXTI->PR = 1 << 2;

                operate_parm.laser_operate_data.standby |= 0x01 << 0;  // 调试模式
                Can_Command_Reply(msg, Success);
            } else if (msg->data.byte[7] == 0xF0)  //
            {
                adaptor_parm.part.laser_module_adaptor_parm.check_feature_1.all = flash_ada_parm.part.laser_module_adaptor_parm.check_feature_1.all;
                adaptor_parm.part.laser_module_adaptor_parm.check_feature_2.all = flash_ada_parm.part.laser_module_adaptor_parm.check_feature_2.all;

                brake_state = 0;

                EXTI->PR = 1 << 2;
                EXTI->IMR |= (1 << 2);

                operate_parm.laser_operate_data.standby &= ~(0x01 << 0);  // 非调试模式

                Can_Command_Reply(msg, Success);

            }  // 关闭调试
            else {
                Can_Command_Reply(msg, Failure);
            }
            break;
        }

        case (0x006):  // 适应性参数存储/初始化
        {
            if (msg->data.byte[7] == 0x0F)  // 参数存储
            {
                configurable_parm_to_flash_parm();
                clear_adaptor_rsvd(&flash_ada_parm);
                flash_write_halfword((unsigned short *)WRITE_START_ADDR, (unsigned short *)&flash_ada_parm, sizeof(flash_ada_parm) / 2);
                adaptor_parm = flash_ada_parm;
                clear_adaptor_rsvd(&adaptor_parm);

                Can_Command_Reply(msg, Success);
            } else if (msg->data.byte[7] == 0xF0)  // 存储参数初始化
            {
                init_ada_parm_and_store(&flash_ada_parm);
                adaptor_parm = flash_ada_parm;
                clear_adaptor_rsvd(&adaptor_parm);
                Can_Command_Reply(msg, Success);
            } else {
                Can_Command_Reply(msg, Failure);
            }
            break;
        }

        case (0x101):  // 运行参数查询
        {
            fill_operate_parm_report();

            if (msg->data.byte[7] == 0x0F) {
                Can_Laser_PARA_Reply(msg, 0x96, (unsigned char *)&operate_parm_report, sizeof(operate_parm_report));
            } else {
                Can_Command_Reply(msg, Failure);
            }
            break;
        }

        case (0x102):  // 适应性参数查询
        {
            if (msg->data.byte[7] == 0x0F) {
                Can_Laser_PARA_Reply(msg, 0x64, (unsigned char *)&adaptor_parm.part.power_output_adaptor_parm, (sizeof(struct adaptor_paramter_bit) - 1));
            } else {
                Can_Command_Reply(msg, Failure);
            }
            break;
        }

        case (0x201):  // 作战参数配置
        {
            if (((operate_parm.laser_operate_data.standby >> 1) & 0x01) || (0 != operate_parm.laser_operate_data.Laser_State))  // 已撤收状态、非空闲状态无法操作
            {
                Can_Command_Reply(msg, Failure);
            } else {
                if (msg->data.byte[7] > 100) {
                    operate_parm.laser_operate_data.light_intensity = 100;
                } else {
                    operate_parm.laser_operate_data.light_intensity = msg->data.byte[7];
                }

                Can_Command_Reply(msg, Success);
            }
            break;
        }
#if (DEVICE_ID == 4)
        case 0x501: {
            seedErrCnt                                  = 0;
            operate_parm.seedSrc_operate_data.seedState = msg->data.byte[7] & 0x7f;
            if ((operate_parm.seedSrc_operate_data.seedState == 2) || (msg->data.byte[7] & 0x80)) {
                operate_parm.laser_operate_data.fault_code2.bit.seedSrcErr = 1;
                Laser_Health_Refresh();
            }
            break;
        }
#endif
        default:
            break;
    }
    /*命令回传*/

    unsigned char j = 0;
    /**/
    if ((0x202 <= Can_Command) && (0x202 + (sizeof(configurable_ada_parm)) / 8 + 1) >= Can_Command) {
        j = Can_Command - 0x202;
        memory_copy((unsigned long long *)(&configurable_ada_parm) + j, &msg->data, 8);
        Can_Command_Reply(msg, Success);
    }

    return;
}

/*复位命令处理*/
Function_State_Typedef Can_Reset_Command_Dipose(void)
{
    Function_State_Typedef ret = Success;

    /*光源故障清除*/

    operate_parm.laser_operate_data.fault_code1.all = 0;
    operate_parm.laser_operate_data.fault_code2.all = 0;
    operate_parm.laser_operate_data.fault_code3.all = 0;

    clear_check_cnt();

    operate_parm.power_operate_parm.modul1_State = 0;
    operate_parm.power_operate_parm.modul2_State = 0;
    operate_parm.power_operate_parm.modul3_State = 0;
    operate_parm.power_operate_parm.modul4_State = 0;

    /**********************************************************
    操作电源进行复位:
    1. 清空接收FIFO数据标识
    2. 等待标识位置位
    3. 时间戳赋值
    4. 重发计数值清零
    5. 重发数据长度置位
    6. 重发命令赋值
    7. 发送数据
    ***********************************************************/
    Response_Wait_Reset.Send_Data[0]  = 0xFE;
    Response_Wait_Reset.Send_Data[1]  = 0x07;
    Response_Wait_Reset.Send_Data[2]  = 0x02;
    Response_Wait_Reset.Send_Data[3]  = 0x11;
    Response_Wait_Reset.Send_Data[4]  = 0x00;
    Response_Wait_Reset.Send_Data_Num = 7;  // 长度为4
    Response_Wait_Reset.Re_Send_Num   = 0x03;
    Response_Wait_Reset.Response_Wait = DISABLE;
    if (uartSendCmd(&Response_Wait_Reset) == Success) {
        operate_parm.laser_operate_data.standby &= ~(1 << 1);  // 未撤收
        operate_parm.laser_operate_data.standby &= ~(1 << 2);  // 冷待机
    } else
        ret = Failure;

    if (readParaQuery() != Success)  // 发送参数设置指令
    {
        ret = Failure;
    } else {
        if ((operate_parm.power_operate_parm.power_state == 1) || (operate_parm.power_operate_parm.power_state == 5) || (operate_parm.power_operate_parm.power_state == 6) ||
            (operate_parm.power_operate_parm.power_state == 7))
            ret = Failure;
    }

    C_FAULT(ON);
    brake_state = 0;
    EXTI->PR    = (1 << 2);
    EXTI->IMR |= (1 << 2);  // 使能刹车中断
    operate_parm.laser_operate_data.Laser_State = 0;
    return ret;
}

/*关闭输出命令处理*/
Function_State_Typedef Can_Close_Control_Command_Dipose(void)
{
    Function_State_Typedef ret = Success;

    Response_Wait_Close_Control.Send_Data_Num = 7;
    Response_Wait_Close_Control.Send_Data[0]  = 0xFE;
    Response_Wait_Close_Control.Send_Data[1]  = 0x07;
    Response_Wait_Close_Control.Send_Data[2]  = 0x03;
    Response_Wait_Close_Control.Send_Data[3]  = 0x22;
    Response_Wait_Close_Control.Send_Data[4]  = 0x00;
    Response_Wait_Close_Control.Response_Wait = DISABLE;
    Response_Wait_Close_Control.Re_Send_Num   = 3;
    ret                                       = uartSendCmd(&Response_Wait_Close_Control);  // 未接收到电源回执，则认为出现电源通信故障

    {
        ULONG start_time = 0;
        start_time       = tx_time_get();
        while (50 > time_span(start_time)) {
            if (operate_parm.laser_operate_data.PD3_Value < adaptor_parm.part.laser_module_adaptor_parm.pd3_low_limit) {
                break;
            }
            tx_thread_relinquish();
        }
        if (operate_parm.laser_operate_data.PD3_Value > adaptor_parm.part.laser_module_adaptor_parm.pd3_low_limit) {
            ret = Failure;
        }
    }
    if (Failure != ret) {
        pd1_check_start = 0;
        if (operate_parm.laser_operate_data.Laser_State != 0x04)  // 不为故障状态
            operate_parm.laser_operate_data.Laser_State = 0x00;   // 若已经出光，则转换为待机状态
    } else {
        operate_parm.laser_operate_data.Laser_State                    = 0x04;
        operate_parm.laser_operate_data.fault_code2.bit.power_comm_err = 1;
        Laser_Health_Refresh();
    }
    return ret;
}

Function_State_Typedef senParaQuery(uint8_t Command)
{
    unsigned char modul1   = 0;
    unsigned char modul2   = 0;
    unsigned char modul3_H = 0;
    unsigned char modul3_L = 0;
    unsigned char modul4_H = 0;
    unsigned char modul4_L = 0;

    switch (Command) {
        case (0x02):  // 出强光
        {
            unsigned short temp;
            modul1 = adaptor_parm.part.power_output_adaptor_parm.strong_modul1;
            modul2 = adaptor_parm.part.power_output_adaptor_parm.strong_modul2;

            temp     = adaptor_parm.part.power_output_adaptor_parm.strong_modul3_L | (adaptor_parm.part.power_output_adaptor_parm.strong_modul3_H << 8);
            temp     = 15 + (temp - 15) * operate_parm.laser_operate_data.light_intensity / 100;
            modul3_H = temp >> 8;
            modul3_L = temp & 0xff;
            temp     = adaptor_parm.part.power_output_adaptor_parm.strong_modul4_L | (adaptor_parm.part.power_output_adaptor_parm.strong_modul4_H << 8);
            temp     = 15 + (temp - 15) * operate_parm.laser_operate_data.light_intensity / 100;
            modul4_H = temp >> 8;
            modul4_L = temp & 0xff;
            break;
        }

        case (0x04):  // 出1级弱光
        {
            modul1   = adaptor_parm.part.power_output_adaptor_parm.level1_modul1;
            modul2   = adaptor_parm.part.power_output_adaptor_parm.level1_modul2;
            modul3_H = adaptor_parm.part.power_output_adaptor_parm.level1_modul3_H;
            modul3_L = adaptor_parm.part.power_output_adaptor_parm.level1_modul3_L;
            modul4_H = adaptor_parm.part.power_output_adaptor_parm.level1_modul4_H;
            modul4_L = adaptor_parm.part.power_output_adaptor_parm.level1_modul4_L;
            break;
        }

        case (0x05):  // 出2级弱光
        {
            modul1   = adaptor_parm.part.power_output_adaptor_parm.level2_modul1;
            modul2   = adaptor_parm.part.power_output_adaptor_parm.level2_modul2;
            modul3_H = adaptor_parm.part.power_output_adaptor_parm.level2_modul3_H;
            modul3_L = adaptor_parm.part.power_output_adaptor_parm.level2_modul3_L;
            modul4_H = adaptor_parm.part.power_output_adaptor_parm.level2_modul4_H;
            modul4_L = adaptor_parm.part.power_output_adaptor_parm.level2_modul4_L;
            break;
        }
        default: {
            return Failure;
        }
    }
    /*电流设置*/
    Response_Wait_PARA_Set.Send_Data[0]  = 0xFE;
    Response_Wait_PARA_Set.Send_Data[1]  = 0x0C;
    Response_Wait_PARA_Set.Send_Data[2]  = 0x01;
    Response_Wait_PARA_Set.Send_Data[3]  = modul1;    // 模块1电流
    Response_Wait_PARA_Set.Send_Data[4]  = modul2;    // 模块2电流
    Response_Wait_PARA_Set.Send_Data[5]  = modul3_H;  // 模块3电流
    Response_Wait_PARA_Set.Send_Data[6]  = modul3_L;  // 模块3电流
    Response_Wait_PARA_Set.Send_Data[7]  = modul4_H;  // 模块4电流
    Response_Wait_PARA_Set.Send_Data[8]  = modul4_L;  // 模块4电流
    Response_Wait_PARA_Set.Send_Data[9]  = 0;         //
    Response_Wait_PARA_Set.Response_Wait = ENABLE;
    Response_Wait_PARA_Set.Send_Data_Num = 0x0C;
    Response_Wait_PARA_Set.Re_Send_Num   = 3;

    return uartSendCmd(&Response_Wait_PARA_Set);  // 发送参数设置指令
}

/* 开启输出命令处理*/
Function_State_Typedef Can_Open_Control_Command_Dipose(uint8_t Command)
{
    if (operate_parm.seedSrc_operate_data.seedState != 1)
        return Failure;
    if (senParaQuery(Command) != Success)
        return Failure;
    /*************************************************************
    开启输出:
    1. 延迟3mS
    2. 清空接收FIFO数据标识
    3. 等待标识位置位
    4. 时间戳赋值
    5. 重发计数值清零
    6. 重发数据长度置位
    7. 重发命令赋值
    8. 发送数据
    *****************************************************************/
    Delay_1ms(3);
    /*命令赋值*/
    Response_Wait_Open_Control.Send_Data[0]  = 0xFE;
    Response_Wait_Open_Control.Send_Data[1]  = 0x07;
    Response_Wait_Open_Control.Send_Data[2]  = 0x03;
    Response_Wait_Open_Control.Send_Data[3]  = 0x11;
    Response_Wait_Open_Control.Send_Data[4]  = 0x00;
    Response_Wait_Open_Control.Send_Data_Num = 7;
    Response_Wait_Open_Control.Response_Wait = DISABLE;
    Response_Wait_Open_Control.Re_Send_Num   = 3;

    if (0x02 == Command)
        Response_Wait_Open_Control.Origin_Timestamp = 0;
    else
        Response_Wait_Open_Control.Origin_Timestamp = 0;
    if (uartSendCmd(&Response_Wait_Open_Control) != Success)  // 发送参数设置指令
        return Failure;

    switch (Command) {
        case (0x02):  // 出强光 状态位置位
        {
            // LONG start_time = tx_time_get();
            // while ((operate_parm.laser_operate_data.PD3_Value == 0) &&
            //        (operate_parm.power_operate_parm.modul1_C_Out < 5) &&
            //        (2000 >= time_span(start_time)))
            //   tx_thread_relinquish();

            // if ((operate_parm.laser_operate_data.PD3_Value > 1) ||
            //     (operate_parm.power_operate_parm.modul1_C_Out >= 5))
            // {
            // }
            operate_parm.laser_operate_data.Laser_State = 0x01;
            return Success;
            break;
        }
        case (0x04):  // 出1级弱光 状态位置位
        {
            operate_parm.laser_operate_data.Laser_State = 0x03;
            return Success;
            break;
        }
        case (0x05):  // 出2级弱光 状态位置位
        {
            operate_parm.laser_operate_data.Laser_State = 0x05;
            return Success;
            break;
        }
        default: {
            break;
        }
    }
    operate_parm.laser_operate_data.fault_code2.bit.power_comm_err = 1;
    Laser_Health_Refresh();
    return Failure;
}

/* 作战准备指令处理*/
Function_State_Typedef Can_Ready_Command_Dipose(uint8_t Command)
{
    Function_State_Typedef ret = Success;
    if (Command == 0x0F)  // 热待机
    {
        operate_parm.laser_operate_data.standby |= 0x01 << 2;  // 热待机 状态
        ret = Success;
    } else  // 冷待机
    {
        operate_parm.laser_operate_data.standby &= ~(0x01 << 2);  // 冷待机 状态
        ret = Success;
    }
    return ret;
}

/* 种子源广播*/
Function_State_Typedef seedReport(uint8_t data)
{
    struct can_message tx_msg;
    memory_set(&tx_msg, 0, sizeof(tx_msg));
    tx_msg.exid = ((uint32_t)1 << 20) + 0x501;
    // tx_msg.exid = (1 << 20) + ((4) << 12) + 0x501;

    // tx_msg.exid = ((uint32_t)adaptor_parm.part.laser_module_adaptor_parm.can_id << 20) | (0x00 << 12) | +0x501;
    tx_msg.data.byte[0] = 0;
    tx_msg.data.byte[1] = 0;
    tx_msg.data.byte[2] = 0;
    tx_msg.data.byte[3] = 0;
    tx_msg.data.byte[4] = 0;
    tx_msg.data.byte[5] = 0;
    tx_msg.data.byte[6] = 2;
    tx_msg.data.byte[7] = data;

    can1_send(&tx_msg);
    return Success;
}

/* 命令回执处理*/
Function_State_Typedef Can_Command_Reply(struct can_message *msg, Function_State_Typedef Can_State)
{
    struct can_message tx_msg;
    memory_set(&tx_msg, 0, sizeof(tx_msg));
    tx_msg.exid = ((uint32_t)adaptor_parm.part.laser_module_adaptor_parm.can_id << 20) + ((msg->exid & 0xff00000) >> 8) + 0x300;

    tx_msg.data.byte[0] = (msg->exid >> 8) & 0xf;
    tx_msg.data.byte[1] = msg->exid & 0xff;   // 操作ID
    tx_msg.data.byte[2] = msg->data.byte[7];  // 操作码

    if (Can_State == Success)
        tx_msg.exid |= 0x0E;
    else
        tx_msg.exid |= 0x0F;

    can1_send(&tx_msg);
    return Success;
}

/* 出光到位回传处理*/
Function_State_Typedef Laser_Open_Success_Reply(unsigned int id)  // 出光到位主动回传
{
    struct can_message tx_msg;

    tx_msg.exid         = (DEVICE_ID << 20) + (id << 12) + 0x310;
    tx_msg.data.byte[0] = operate_parm.laser_operate_data.Laser_State;
    tx_msg.data.byte[1] = operate_parm.laser_operate_data.light_intensity;
    can1_send(&tx_msg);

    return Success;
}

/* 参数回传处理*/
Function_State_Typedef Can_Laser_PARA_Reply(struct can_message *msg, uint16_t Command, uint8_t *p, uint8_t PARA_Num)
{
    struct can_message tx_msg;
    uint8_t            i   = 0;
    uint8_t            len = 0;
    tx_msg.exid            = (DEVICE_ID << 20) + ((msg->exid & 0xff00000) >> 8) + Command;

    while (PARA_Num) {
        len = PARA_Num < 8 ? PARA_Num : 8;
        for (i = 0; i < len; i++) {
            tx_msg.data.byte[i] = *(p++);
            PARA_Num--;
        }

        can1_send(&tx_msg);
        tx_msg.exid++;
    }
    return Success;
}

int can1_send(struct can_message *msg)
{
    ULONG         start_time  = 0;
    unsigned char re_send_cnt = 3;
    unsigned char tx_status   = 0;
    unsigned char fail_flag   = 0;
    int           ret         = 0;
    tx_mutex_get(&mutex_can_dev, TX_WAIT_FOREVER);
    if (!(CAN1->TSR & (1 << 26))) {
        CAN1->TSR |= (1 << 7);
        start_time = tx_time_get();
        while ((!(CAN1->TSR & (1 << 0))) && (50 >= time_span(start_time)))
            tx_thread_relinquish();
    }

    CAN1->sTxMailBox[0].TDTR = 8;

    /* Set up the data field */
    CAN1->sTxMailBox[0].TDLR = msg->data.word[0];
    CAN1->sTxMailBox[0].TDHR = msg->data.word[1];
    CAN1->sTxMailBox[0].TIR  = (msg->exid << 3) | (1 << 2);

    re_send_cnt = 3;
    while (re_send_cnt--) {
        if (!(CAN1->TSR & (1 << 26))) {
            CAN1->TSR |= (1 << 7);
            start_time = tx_time_get();
            while ((!(CAN1->TSR & (1 << 0))) && (50 >= time_span(start_time))) {
                tx_thread_relinquish();
            }
        }

        CAN1->sTxMailBox[0].TDLR = msg->data.word[0];
        CAN1->sTxMailBox[0].TDHR = msg->data.word[1];
        CAN1->sTxMailBox[0].TIR  = (msg->exid << 3) | (1 << 2);
        CAN1->sTxMailBox[0].TDTR = 8;

        CAN1->sTxMailBox[0].TIR |= (1 << 0);
        start_time = tx_time_get();

        tx_status = (CAN1->TSR & (1 << 0)) && (CAN1->TSR & (1 << 1)) && (CAN1->TSR & (1 << 26)) && (!(CAN1->sTxMailBox[0].TIR & 1));
        fail_flag = CAN1->TSR & (1 << 3);
        while ((!tx_status) && (150 > time_span(start_time)) && (!fail_flag)) {
            tx_thread_relinquish();
            tx_status = (CAN1->TSR & (1 << 0)) && (CAN1->TSR & (1 << 1)) && (CAN1->TSR & (1 << 26)) && (!(CAN1->sTxMailBox[0].TIR & 1));
            fail_flag = CAN1->TSR & (1 << 3);
        }

        tx_status = (CAN1->TSR & (1 << 0)) && (CAN1->TSR & (1 << 1)) && (CAN1->TSR & (1 << 26)) && (!(CAN1->sTxMailBox[0].TIR & 1));
        if (tx_status) {
            ret = 0;
            break;
        } else {
            ret = -1;
        }
    }

    CAN1->TSR |= (1 << 7);
    tx_mutex_put(&mutex_can_dev);
    return ret;
}

void upload_operate_parm_thread(ULONG arg)
{
    struct can_message msg;
    msg.exid = 1 << 20;
    tx_timer_activate(&timer_upload_operate_parm);
    while (1) {
        tx_semaphore_get(&semaphore_upload_operate_parm, TX_WAIT_FOREVER);
        fill_operate_parm_report();
        Can_Laser_PARA_Reply(&msg, 0x096, (unsigned char *)&operate_parm_report, sizeof(operate_parm_report));
    }
}

void can1_rx0_thread(ULONG arg)
{
    struct can_message rx_msg;
    while (1) {
        while (can1_rx0_buf.head != can1_rx0_buf.tail) {
            rx_msg = can1_rx0_buf.buf[can1_rx0_buf.head];
            can_protocol_parse(&rx_msg);
            can1_rx0_buf.head++;
            if (CAN1_RX0_BUF_LEN <= can1_rx0_buf.head)
                can1_rx0_buf.head = 0;
        }

        tx_thread_relinquish();
    }
}

void state_manage_thread(ULONG arg)
{
    ULONG current_time = tx_time_get();
    while (1) {
        Laser_Health_Refresh();
        if ((0x4 == operate_parm.laser_operate_data.Laser_State) && (0 == err_dispose_finish)) {
            if (5000 <= time_span(current_time)) {
                current_time                                 = tx_time_get();
                Response_Wait_Close_Control.Send_Data_Num    = 7;  // 长度为4
                Response_Wait_Close_Control.Send_Data[0]     = 0xFE;
                Response_Wait_Close_Control.Send_Data[1]     = 0x07;
                Response_Wait_Close_Control.Send_Data[2]     = 0x03;
                Response_Wait_Close_Control.Send_Data[3]     = 0x22;
                Response_Wait_Close_Control.Send_Data[4]     = 0x00;
                Response_Wait_Close_Control.Origin_Timestamp = 3;  // 停光指令

                if (Success == Uart_Send_Command(&Response_Wait_Close_Control)) {
                    if (Success == Can_Ready_Command_Dipose(0xF0))
                        err_dispose_finish = 1;
                }
            }
        }
        tx_thread_relinquish();
    }
}

void USB_LP_CAN_RX0_IRQHandler(void)
{
    can1_rx0_buf.buf[can1_rx0_buf.tail].exid         = CAN1->sFIFOMailBox[0].RIR >> 3;
    can1_rx0_buf.buf[can1_rx0_buf.tail].data.word[0] = CAN1->sFIFOMailBox[0].RDLR;
    can1_rx0_buf.buf[can1_rx0_buf.tail].data.word[1] = CAN1->sFIFOMailBox[0].RDHR;

    CAN1->RF0R |= CAN_RF0R_RFOM0;
    can1_rx0_buf.tail++;
    if (CAN1_RX0_BUF_LEN <= can1_rx0_buf.tail)
        can1_rx0_buf.tail = 0;
}

void CAN_RX1_IRQHandler(void)
{
    can1_rx1_buf.buf[can1_rx1_buf.tail].exid         = CAN1->sFIFOMailBox[1].RIR >> 3;
    can1_rx1_buf.buf[can1_rx1_buf.tail].data.word[0] = CAN1->sFIFOMailBox[1].RDLR;
    can1_rx1_buf.buf[can1_rx1_buf.tail].data.word[1] = CAN1->sFIFOMailBox[1].RDHR;

    CAN1->RF1R |= CAN_RF1R_RFOM1;
    can1_rx1_buf.tail++;
    if (CAN1_RX1_BUF_LEN <= can1_rx1_buf.tail)
        can1_rx1_buf.tail = 0;
}
